#include <opencv2/opencv.hpp>
#include "MoveSenseCamera.h"

#ifdef _WIN32
#include <direct.h>
#include <io.h>
#else
#include <stdio.h>
#include <unistd.h> 
#include <sys/stat.h>
#endif

#ifdef _WIN32
#define ACCESS _access
#define MKDIR(a) _mkdir((a))
#else 
#define ACCESS access
#define MKDIR(a) mkdir((a),0755)
#endif

using namespace movesense;

MoveSenseCamera *camera;

void on_Trackbar_DesiredBrightness(int value, void *)  
{  
	camera->SetDesiredBringhtness(value);
}  

void on_Trackbar_Exposure(int value, void *)  
{  
	camera->SetExposure(value);
}  

void on_Trackbar_Gain(int value, void *)  
{  
	camera->SetGain(value);
}  

void colorDisparity( const cv::Mat msrc, cv::Mat &mdst, int nmax=120 );

int main()
{
	bool rectify_sw = true;

	bool auto_exposure = true;

	int desired_brightness = 70;
	int min_exposure = 1;
	int min_gain = 16;
	int max_exposure = 255;
	int max_gain = 1024;

	int max_brightness = 255;

	int current_exposure = 70;
	int current_gain = 70;

	camera = new MoveSenseCamera();
	if (!camera->IsAvailable())
	{
		cout << "No camera connected..." << endl;
		return 0;
	}

	//CAMERA_LR  CAMERA_LD  CAMERA_LRD 
	int mode = CAMERA_LRD;
	int res = camera->SetMode(mode);
	if (res != MS_SUCCESS)
	{
		cout << "The mode is not supported..." << endl;
		return 0;
	}
	
	if (camera->Open() != MS_SUCCESS)
	{
		cout << "Open camera failed!" << endl;
		camera->Close();
		delete camera;
		return 0;
	}
	cout << "Open camera ok: " << camera->GetName() << endl;

	camera->StartStream();
	camera->SetStereoRectify(rectify_sw);

	//Get camera setting
	Resolution resolution = camera->GetResolution();
	int w = resolution.width;
	int h = resolution.height;
	int len = camera->GetImageDataLength();
	int bitDepth = camera->GetBitDepth();
	if( bitDepth != 8 && bitDepth != 16 )
	{
		cout << "bitDepth illegal (should be 8 or 16)!" << endl;
		camera->StopStream();
		camera->Close();
		delete camera;
		return MS_FAIL;
	}
	
	cv::Mat left(h,w,CV_8UC1),right(h,w,CV_8UC1),disparity(h,w,CV_8UC1);
	cv::Mat depth(h,w,CV_16UC1),depthShow(h,w,CV_8U),depthShowColor(h,w,CV_8UC3);

	camera->SetAutoExposure(auto_exposure);
	camera->SetAutoGain(auto_exposure);
	camera->SetMaxExposure(max_exposure);
	camera->SetMinExposure(min_exposure);
	camera->SetMaxGain(max_gain);
	camera->SetMinGain(min_gain);
	camera->SetDesiredBringhtness(desired_brightness);

	if (!auto_exposure)
	{
		camera->SetExposure(current_exposure);
		camera->SetGain(current_gain);
	}

	cv::namedWindow("left");
	cv::createTrackbar("exposure", "left", &current_exposure, max_exposure, on_Trackbar_Exposure); 

	cv::namedWindow("right");
	cv::createTrackbar("gain", "right", &current_gain, max_gain, on_Trackbar_Gain); 

	if( mode == CAMERA_LD || mode == CAMERA_LRD )
	{
	    if( bitDepth == 16 )
	    {
			cv::namedWindow("depth");
			cv::createTrackbar("desired brightness", "depth", &desired_brightness, max_brightness, on_Trackbar_DesiredBrightness); 
		}
		else
		{
			cv::namedWindow("disp");
			cv::createTrackbar("desired brightness", "disp", &desired_brightness, max_brightness, on_Trackbar_DesiredBrightness); 
		}
	}

	int imgIndex = 0;

	if (-1 == ACCESS("image", 0))  MKDIR("image");

	int frameCnt = 0;
	double time1=cv::getTickCount(), time2=0, time=0;

	while (1)
	{
		if( bitDepth == 16 )
		{
			res = camera->GetImageData(left.data, right.data, depth.data, len);
		}
		else
		{
			res = camera->GetImageData(left.data, right.data, disparity.data, len);
		}

		if(res == MS_SUCCESS)
		{
			frameCnt++;

			if (frameCnt == 50)
			{
				time2 = cv::getTickCount();
				time = (time2-time1)/cv::getTickFrequency();
				cout << "fps: " << (frameCnt)/time << endl;

				frameCnt = 0;
				time1 = time2;
			}

			cv::imshow("left", left);
			if( mode != CAMERA_LD )
			{
				cv::imshow("right", right);  
			}

			if( mode == CAMERA_LD || mode == CAMERA_LRD )
			{
				if( bitDepth == 16 )
				{
					for(int j = 0 ; j < w*h; j++)							   
					{
						depthShow.data[j]	= depth.data[j*2 + 1];
					}
					cv::imshow("depth", depthShow);
					colorDisparity(depthShow, depthShowColor);
					cv::imshow("depthShowColor", depthShowColor);
				}
				else
				{
					cv::imshow("disp", disparity);
				}
			}

			int key = cv::waitKey(5);
			if (key == 'q' || key == 'Q')
			{
				cv::destroyAllWindows();
				break;
			}
			else if (key == 'r' || key == 'R')
			{
				rectify_sw = !rectify_sw;
				camera->SetStereoRectify(rectify_sw);
				if (rectify_sw)
				{
					cout << "SetStereoRectify: on" << endl;
				}
				else
				{
					cout << "SetStereoRectify: off" << endl;
				}
			}
			else if (key == ' ')
			{
				char left_name[100], right_name[100], disp_name[100];
				sprintf(left_name, "image\\left_%d.png", imgIndex);
				cv::imwrite(left_name, left);
				if( mode != CAMERA_LD )
				{
					sprintf(right_name, "image\\right_%d.png", imgIndex);
					cv::imwrite(right_name, right);
				}
				
				if( mode == CAMERA_LD || mode == CAMERA_LRD )
				{
					if( bitDepth == 16 )
					{
						sprintf(disp_name, "image\\disp_%d.png", imgIndex);
						cv::imwrite(disp_name, depth);
					}
					else
					{
						sprintf(disp_name, "image\\depth_%d.png", imgIndex);
						cv::imwrite(disp_name, disparity);
					}
				}
				imgIndex++;
			}
			else if (key == 'e' || key == 'E')
			{
				auto_exposure = !auto_exposure;
				camera->SetAutoExposure(auto_exposure);
				camera->SetAutoGain(auto_exposure);
			}
		}
		else if (res == MS_TIMEOUT)
		{
			cout << "GetImageData: timeout" << endl;
		}
		else
		{
			cout << "GetImageData: failed" << endl;
		}
	}

	camera->StopStream();
	camera->Close();
	delete camera;

	return MS_SUCCESS;
}

void colorDisparity( const cv::Mat msrc, cv::Mat &mdst, int nmax )
{
	double ninterval = 300./nmax;
	int alpha = nmax  * 0.8;
	mdst.create(msrc.size(), CV_8UC3);
	int ar[] = {  0, 119, 119, 110, 110, 102, 102, 93, 93, 85, 85, 76, 76, 68,
		68, 59, 59, 51, 51, 42, 42, 34, 34, 25, 25, 17, 17, 8, 8, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0, 0, 8, 8, 17, 17, 25, 25, 34, 34, 42, 42,
		51, 51, 59, 59, 68, 68, 76, 76, 85, 85, 93, 93, 102, 102, 110,
		110, 119, 119, 127, 127, 135, 135, 144, 144, 152, 152, 161, 161,
		169, 169, 178, 178, 186, 186, 195, 195, 203, 203, 212, 212, 220,
		220, 229, 229, 237, 237, 246, 246, 254, 254, 254, 254, 254, 254,
		254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
		254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
		254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
		254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
		254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
		254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
		254, 254, 254, 254, 254, 254, 254, 254 };
	int ag[] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 8, 8, 17, 17, 25, 25, 34, 34, 42,
		42, 51, 51, 59, 59, 68, 68, 76, 76, 85, 85, 93, 93, 102, 102, 110,
		110, 119, 119, 127, 127, 135, 135, 144, 144, 152, 152, 161, 161,
		169, 169, 178, 178, 186, 186, 195, 195, 203, 203, 212, 212, 220,
		220, 229, 229, 237, 237, 246, 246, 254, 254, 254, 254, 254, 254,
		254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
		254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
		254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
		254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
		254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
		254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
		254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
		254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
		254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 246,
		246, 237, 237, 229, 229, 220, 220, 212, 212, 203, 203, 195, 195,
		186, 186, 178, 178, 169, 169, 161, 161, 152, 152, 144, 144, 135,
		135, 127, 127, 119, 119, 110, 110, 102, 102, 93, 93, 85, 85, 76,
		76, 68, 68, 59, 59, 51, 51, 42, 42, 34, 34, 25, 25, 17, 17, 8, 8,
		0, 0, 0, 8, 8, 17, 17, 25, 25, 34, 34, 42, 42, 51, 51, 59, 59, 68,
		68, 76, 76, 85, 85, 93, 93, 102, 102, 110, 110, 119, 119, 127 };
	int ab[] = {  0, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
		254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
		254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
		254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
		254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
		254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
		254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254,
		246, 246, 237, 237, 229, 229, 220, 220, 212, 212, 203, 203, 195,
		195, 186, 186, 178, 178, 169, 169, 161, 161, 152, 152, 144, 144,
		135, 135, 127, 127, 119, 119, 110, 110, 102, 102, 93, 93, 85, 85,
		76, 76, 68, 68, 59, 59, 51, 51, 42, 42, 34, 34, 25, 25, 17, 17, 8,
		8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };

	for ( int _row=0; _row<msrc.rows; _row++ )
	{
		for ( int _col=0; _col<msrc.cols; _col++ )
		{
			int r,g,b;
			int disp = msrc.at<uchar>(_row, _col);
			if ( disp > nmax )
			{
				r = ar[300];
				g = ag[300];
				b = ab[300];
			}
			else
			{
				int idx = (int) disp*ninterval;
				r = ar[ idx ];
				g = ag[ idx ];
				b = ab[ idx ];
			}
			mdst.at<cv::Vec3b>(_row, _col) = cv::Vec3b(b,g,r);	
		}
	}

}

